#include "ModBus/ModBusProcess.h"
#include "Log/Logger.h"
#include "Publisher_profile.h"
#include "Subscriber_profile.h"

ModBusProcess::ModBusProcess(/* args */) : mQuit(false),
                                           mDataQueue(nullptr),
                                           pub(nullptr)
{
    mDataQueue = new SyncQueue<MODBUS::CollectData>(2000);
}

ModBusProcess::~ModBusProcess()
{
    if (mDataQueue)
    {
        delete mDataQueue;
        mDataQueue = nullptr;
    }
    if (pub)
    {
        delete pub;
        pub = nullptr;
    }
    if (sub)
    {
        delete sub;
        sub = nullptr;
    }
    mQuit.store(true);
}

void ModBusProcess::init()
{
    std::string xml_path = "/home/wang/Fast-DDS-Dev/dds/profile_example/fastdds_example.xml";
    std::string participant_profile = "participant_example_profile";
    std::string topic_profile = "topic_example_profile";
    std::string datareader_profile = "datareader_example_profile";
    std::string datawriter_profile = "datawriter_example_profile";
    std::string topic_name = "modbus/power";

    pub = new Publisher_profile();
    if (!pub)
    {
        Logger::getInstance().error("DDS publisher creates failed...");
        return;
    }
    bool res = pub->init(xml_path, participant_profile, topic_profile, datawriter_profile, topic_name);
    if (!res)
    {
        delete pub;
        pub = nullptr;
        Logger::getInstance().error("DDS publisher init failed...");
        return;
    }

    topic_name = "CSS/100ms";
    sub = new Subscriber_profile();
    if (!sub)
    {
        Logger::getInstance().error("DDS sucscriber creates failed...");
        return;
    }
    sub->init(xml_path, participant_profile, topic_profile, datareader_profile, topic_name, [this](Message msg)
              { this->onPublish(msg); });
}

void ModBusProcess::start()
{
    startTimer();
    process();
}

void ModBusProcess::onPublish(Message m)
{
    std::vector<uint8_t> sendData;

    std::vector<uint8_t> timeS = m.getByteData();

    // CSS发出来的只有8字节
    if (timeS.size() != 8)
    {
        Logger::getInstance().error("css time is error, the byte size is: " + std::to_string(timeS.size()));
        return;
    }
    double cssTime;
    std::memcpy(&cssTime, timeS.data(), sizeof(double));
    uint64_t unixTime = cssTime * 1000 - 2082844800000;

    sendData.push_back(1);                                               // 状态，暂时设为1
    std::copy(timeS.begin(), timeS.end(), std::back_inserter(sendData)); // css时间

    std::map<int, std::queue<double>> tempData;
    {
        std::unique_lock<std::mutex> lock(data_mutex);
        tempData = mData;
        mData.clear();
    }
    for (auto it = tempData.begin(); it != tempData.end(); it++)
    {
        sendData.push_back((uint8_t)it->first); // 序号
        sendData.push_back((uint8_t)9);         // 数据类型
        Int2Byte((int)it->second.size(), sendData); //数据长度
        auto q = it->second;
        while (!q.empty())
        {
            double value = q.front();
            q.pop();
            Double2Byte(value, sendData);
        }
    }

    // std::this_thread::sleep_for(std::chrono::milliseconds(100));
    Message msg;
    msg.setByteData(sendData);
    msg.message("data");
    msg.index(m.index());
    pub->publish("modbus/power", msg);
    std::cout << "send" << std::endl;
}

void ModBusProcess::startTimer()
{
    /*std::thread([this]()
    {
        long i = 0;
        while(!mQuit.load())
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            Message msg;
            msg.message("data");
            msg.index(i++);
            pub->publish("CSS/100ms", msg);
        }
    }).detach();*/
}

void ModBusProcess::stop()
{
    mQuit.store(true);
    mDataQueue->exit();
}

void ModBusProcess::update(const MODBUS::CollectData &data)
{
    mDataQueue->Put(data);
    Logger::getInstance().info("mDataQueue.size() " + std::to_string(mDataQueue->Size()));
}

void ModBusProcess::process()
{
    while (!mQuit.load())
    {
        MODBUS::CollectData data;
        mDataQueue->Take(data);
        if (mQuit.load())
            return;
        for (int i = 0; i < 3; i++)
        {
            {
                std::unique_lock<std::mutex> lock(data_mutex);
                mData[i].push(data.rawData[i]);
            }
            Logger::getInstance().info("Register " + std::to_string(i) + ": " + std::to_string(data.rawData[i]));
        }
    }
}

void ModBusProcess::Int2Byte(int value, std::vector<uint8_t> &d)
{
    unsigned char* bytePtr = reinterpret_cast<uint8_t*>(&value);
    for(int i = sizeof(int) - 1; i >= 0; i--)
    {
        d.push_back(bytePtr[i]);
    }
}
void ModBusProcess::Double2Byte(double value, std::vector<uint8_t> &d)
{
    unsigned char* bytePtr = reinterpret_cast<uint8_t*>(&value);
    for(int i = sizeof(double) - 1; i >= 0; i--)
    {
        d.push_back(bytePtr[i]);
    }
}